function [U,Uxy] = quadrotor_controller(t, x, Uxy, Uxy2)


%% 状态参数
PHI   = x(1); PHI_dot   = x(2); 
THETA = x(3); THETA_dot = x(4);
PSI   = x(5); PSI_dot   = x(6);

Z = x(7);  Z_dot = x(8);
X = x(9);  X_dot = x(10);
Y = x(11); Y_dot = x(12);

%% DesirePosition
[DesPos, DesAtt] = quadrotor_desiredtrajectory(t);

Xd = DesPos(4);
Yd = DesPos(7);
Zd = DesPos(1);

Xd_dot = DesPos(5);
Yd_dot = DesPos(8);
Zd_dot = DesPos(2);

Xd_2dot = DesPos(6);
Yd_2dot = DesPos(9);
Zd_2dot = DesPos(3);

%% DesireAttitude
PHId   = DesAtt(1);
THETAd = DesAtt(4);
PSId   = DesAtt(7);

PHId_dot   = DesAtt(2);
THETAd_dot = DesAtt(5);
PSId_dot   = DesAtt(8);

PHId_2dot   = DesAtt(3);
THETAd_2dot = DesAtt(6);
PSId_2dot   = DesAtt(9);

%% Parameter Definition 
global m   g   dt
global a1  a2  a3
global b1  b2  b3
global c1  c2  c3  c4
global c5  c6  c7  c8
global c9  c10 c11 c12
kz =1;
%% Z Control
% z7 = Zd - Z;
% z7_dot = Zd_dot - Z_dot;
% z8 = -z7_dot - c7*z7 ;
% U1 =m/cos(PHI)/cos(THETA)*(z7 + g + Zd_2dot - c8*z8 + c7*z7_dot);
z7 = Z - Zd;
z7_dot = Z_dot - Zd_dot;
z8 = z7_dot + c7*z7 ;
U1 =m/cos(PHI)/cos(THETA)*(-z7 + g + Zd_2dot - c8*z8 - c7*z7_dot);

%% X - THETA Control
%% X Control
% z9  = Xd - X;
% z10 = X_dot - Xd_dot - c9*z9;
% Ux = m/U1*(z9 + Xd_2dot - c10*z10 + c9*(Xd_dot - X_dot));
z9  = X - Xd;
z9_dot = X_dot - Xd_dot;
z10 = z9_dot + c9*z9;
Ux = m/U1*(-z9 + Xd_2dot - c10*z10 - c9*z9_dot);
Ux_dot = (Ux - Uxy(1))/dt;        % Ux近似导数
Ux_2dot = (Ux - Uxy2(1))/2/dt;    % Ux近似二阶导数

%% 近似PSI=0;原式：
% Ux = cos(PHI)*cos(THETA)*cos(PSI) + sin(PHI)*sin(PSI) -> PHId = arcsin(Ux*sin(PSI) - Uy*cos(PSI))
% Uy = cos(PHI)*sin(THETA)*sin(PSI) - sin(PHI)*cos(PSI) -> THETAd = arcsin(Ux/cos(PHId)/cos(PSI) - sin(PHId)*sin(PSI)/cos(PHId)/cos(PSI));
% PHId   = -arcsin(Uy) -> PHId   = -Uy
% THETAd = arcsin(Ux)  -> THETAd = Ux;

%% THETA Control
THETAd = Ux; 
THETAd_dot = Ux_dot;       
THETAd_2dot = Ux_2dot;

% z3 = THETAd - THETA;
% z3_dot = THETAd_dot - THETA_dot;
% z4 = -z3_dot - c3*z3;
% U3 = 1/b2*(z3 - a2*PHI_dot*PSI_dot + THETAd_2dot - c4*z4 + c3*z3_dot);
z3 = THETA - THETAd;
z3_dot = THETA_dot - THETAd_dot;
z4 = z3_dot + c3*z3;
U3 = 1/b2*(-z3 - a2*PHI_dot*PSI_dot + THETAd_2dot - c4*z4 - c3*z3_dot);

%% Y - PHI Control
%% Y Control
% z11 = Yd - Y;
% z12 = Y_dot - Yd_dot - c11*z11;
% Uy = m/U1*(z11 + Yd_2dot - c12*z12 + c11*(Yd_dot - Y_dot));
% Uy_dot = (Uy - Uxy(2))/dt;        % Uy近似导数
% Uy_2dot = (Uy - Uxy2(2))/2/dt;  % Uy近似二阶导数

z11 = Y - Yd;
z11_dot = Y_dot - Yd_dot;
z12 = z11_dot + c11*z11;
Uy = m/U1*(-z11 + Yd_2dot - c12*z12 - c11*z11_dot);
Uy_dot = (Uy - Uxy(2))/dt;        % Uy近似导数
Uy_2dot = (Uy - Uxy2(2))/2/dt;  % Uy近似二阶导数
%% PHI Control
PHId = -Uy;
PHId_dot = -Uy_dot;
PHId_2dot = -Uy_2dot;
% z1 = PHId - PHI;
% z2 = PHI_dot - PHId_dot - c1*z1;
% U2 = 1/b1*(z1 - a1*THETA_dot*PSI_dot + PHId_2dot - c2*z2 + c1*(PHId_dot - PHI_dot));
z1 = PHI - PHId;
alpha1 = PHId_dot - c1*z1;
z1_dot = PHI_dot - PHId_dot;
alpha1_dot = PHId_2dot - c1*z1_dot;
z2 = PHI_dot - alpha1;
U2 = 1/b1*(-z1 - a1*THETA_dot*PSI_dot + alpha1_dot- c2*z2);

% z2 = z1_dot + c1*z1;
% U2 = 1/b1*(-z1 - a1*THETA_dot*PSI_dot + PHId_2dot - c2*z2 - c1*z1_dot);
%% KSI Control
% z5 = PSId - PSI;
% z6 = PSI_dot - PSId_dot - c5*z5;
% U4 = 1/b3*(z5 - a3*PHI_dot*THETA_dot + PSId_2dot - c6*z6 + c5*(PSId_dot - PSI_dot));
z5 = PSI - PSId;
z5_dot = PSI_dot - PSId_dot;
z6 = z5_dot + c5*z5;
U4 = 1/b3*(-z5 - a3*PHI_dot*THETA_dot + PSId_2dot - c6*z6 - c5*z5_dot);
%% Control Inputs
U = [U1; U2; U3; U4];
Uxy = [Ux; Uy];

end